#!/usr/bin/env python
PACKAGE = "qm_wbc"
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("kp_swing", double_t, 0, "Kp of swing leg", 350, 0.0, 500)
gen.add("kd_swing", double_t, 0, "Kd of swing leg", 37, 0.0, 500)

gen.add("baseHeightKp", double_t, 0, "Kp of base height", 400, 0.0, 500)
gen.add("baseHeightKd", double_t, 0, "Kd of base height", 140, 0.0, 500)

gen.add("kp_base_linear", double_t, 0, "Kp of base linear", 400, 0.0, 500)
gen.add("kd_base_linear", double_t, 0, "Kd of base linear", 100, 0.0, 500)

gen.add("kp_base_angular", double_t, 0, "Kp of base angular", 400, 0.0, 500)
gen.add("kd_base_angular", double_t, 0, "Kd of base angular", 140, 0.0, 500)

gen.add("kp_arm_joint_1", double_t, 0, "Kp of arm joint 1", 4000, 0.0, 20000)
gen.add("kp_arm_joint_2", double_t, 0, "Kd of arm joint 2", 4200, 0.0, 20000)
gen.add("kp_arm_joint_3", double_t, 0, "Kp of arm joint 3", 4000, 0.0, 20000)
gen.add("kp_arm_joint_4", double_t, 0, "Kd of arm joint 4", 4000, 0.0, 20000)
gen.add("kp_arm_joint_5", double_t, 0, "Kp of arm joint 5", 4200, 0.0, 20000)
gen.add("kp_arm_joint_6", double_t, 0, "Kd of arm joint 6", 6000.0, 0.0, 20000)

gen.add("kd_arm_joint_1", double_t, 0, "Kd of arm joint 1", 75, 0.0, 20000)
gen.add("kd_arm_joint_2", double_t, 0, "Kd of arm joint 2", 75, 0.0, 20000)
gen.add("kd_arm_joint_3", double_t, 0, "Kd of arm joint 3", 75, 0.0, 20000)
gen.add("kd_arm_joint_4", double_t, 0, "Kd of arm joint 4", 75, 0.0, 20000)
gen.add("kd_arm_joint_5", double_t, 0, "Kd of arm joint 5", 75, 0.0, 20000)
gen.add("kd_arm_joint_6", double_t, 0, "Kd of arm joint 6", 75, 0.0, 20000)

gen.add("kp_ee_linear_x", double_t, 0, "Kp of arm linear", 3000.0, 0.0, 20000) # 6000
gen.add("kp_ee_linear_y", double_t, 0, "Kp of arm linear", 3000.0, 0.0, 20000)
gen.add("kp_ee_linear_z", double_t, 0, "Kp of arm linear", 3000.0, 0.0, 20000)

gen.add("kd_ee_linear_x", double_t, 0, "Kd of arm linear", 75.0, 0.0, 20000) # 100
gen.add("kd_ee_linear_y", double_t, 0, "Kd of arm linear", 75.0, 0.0, 20000)
gen.add("kd_ee_linear_z", double_t, 0, "Kd of arm linear", 75.0, 0.0, 20000)

gen.add("kp_ee_angular_x", double_t, 0, "Kp of arm angular", 2000.0, 0.0, 20000)
gen.add("kp_ee_angular_y", double_t, 0, "Kd of arm angular", 2000.0, 0.0, 20000)
gen.add("kp_ee_angular_z", double_t, 0, "Kp of arm angular", 2000.0, 0.0, 20000)

gen.add("kd_ee_angular_x", double_t, 0, "Kd of arm angular", 75, 0.0, 20000)
gen.add("kd_ee_angular_y", double_t, 0, "Kd of arm angular", 75, 0.0, 20000)
gen.add("kd_ee_angular_z", double_t, 0, "Kd of arm angular", 75, 0.0, 20000)

gen.add("d_ee_x", double_t, 0, "desired linear x of arm", 0.57, -1, 1) # 0.52
gen.add("d_ee_y", double_t, 0, "desired linear y of arm", 0.09, -1, 1)
gen.add("d_ee_z", double_t, 0, "desired linear z of arm", 0.41, -1, 1) # 0.44

gen.add("da_ee_z", double_t, 0, "desired angular z of arm", -1.57, -3.14, 3.14) # -1.57
gen.add("da_ee_y", double_t, 0, "desired angular y of arm", 0, -3.14, 3.14)
gen.add("da_ee_x", double_t, 0, "desired angular x of arm", -1.57, -3.14, 3.14) # -1.57

exit(gen.generate(PACKAGE, "wbcweight", "WbcWeight"))
